/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/*
*  \class	TGroundModelMap
*  \author 	Jari Saarinen
*  \date 	Thu Jun 15 11:59:04 2006

*  \version: 	0.1  
*/
#include <stdlib.h>
#include <math.h>
#include "TGroundModelMap.h"




TGroundModelMap::TGroundModelMap() : TMetricGrid()
{
		xMinConst=-1000;
		xMaxConst=1000;
		yMinConst=-1000;
		yMaxConst=1000;
		zMinConst=-1000;
		zMaxConst=1000;
}

TGroundModelMap::TGroundModelMap(float size_x, float size_y, float reso, TPixelCoord origin) : TMetricGrid(){
	this->initGroundModelMap(size_x,size_y,reso,origin);
}

void TGroundModelMap::initGroundModelMap(float size_x, float size_y, float reso, TPixelCoord origin){
	this->initMetricGrid(size_x, size_y,  reso, origin);
	this->setGroundModelMapToInitialValue(-1);
}

TGroundModelMap::~TGroundModelMap()
{
	
}

/**
* Set the whole map into some initial value 
* @param value	The value that is copied to whole map
*/
void TGroundModelMap::setGroundModelMapToInitialValue(int value){
	TBasePixel p;
	int i,j;

	p.v = value;
	
	for(i=0;i<(int) this->Ny;i++){
		for(j=0;j<(int) this->Nx;j++){
			this->setPixel(p,j,i);
		}
	}

}
/**
* get height value from the map
* @param P	The metric position of the height
* @return The height in that position in meters 
*/
float TGroundModelMap::getHeight(TCoord2D P){
	TBasePixel pix;

	pix = this->getValue(P);

	return ( ((float) pix.v) / 100.0f );
}

/**
* Check if the index is out of bounds
**/
bool TGroundModelMap::isIndexOutOfBounds(int x, int y){
		return (x<0 || x>=this->Nx || y<0 || y>=this->Ny);
}

/**
* Returns the height from the grid position P (in pixels) 
* @param P	the pixel coord 
* @return the height from ther pixel coord P
*/
float TGroundModelMap::getHeight(TPixelCoord P){
	TBasePixel pix;
	
	if(P.x > this->Nx || P.y > this->Ny){
		fprintf(stderr,"TGroundModelMap::getHeight:: Invalid pixel (%d,%d)\n",P.x,P.y);
	}
	
	pix = this->getPixel(P.x,P.y);
	return ( ((float) pix.v) / 100.0f );
}

/**
* Set height @h value to the map into some metric position @P
* @param P the metric position
* @param h the height value
* 
*/ 

int TGroundModelMap::setHeight(TCoord2D P, float height){
	TBasePixel pix;

	pix.v = (int) (height * 100.0f);	// The value is trasformed into CM's for storage
	this->setValue(P,pix);

	return 0;
}

/**
* Set height @h value to the map into grid position TPixelCoord @P
* @param P the pixel coordinate
* @param h the height value
* 
*/ 
int TGroundModelMap::setHeight(TPixelCoord P, float height){
	TBasePixel pix;

	pix.v = (int) (height * 100.0f);	// The value is trasformed into CM's for storage
	this->setPixel(pix,P.x,P.y);

	return 0;
}


/**
* Updates the value to the map. If there already is a value the new value is an average of 
* old and new value.
* @param P		The position of the updated height
* @param height	The height to be updated
* @return the updated value	
*/
float TGroundModelMap::updateHeight(TCoord2D P, float height){
	TBasePixel pix;
	float h;

	
	pix = this->getValue(P);
	if(pix.v == -1){
		h=height;
	}
	else{
		h = this->getHeight(P);
		h = (h + height) * 0.5f;
	}
	
	this->setHeight(P,h);

	return h;
}

/**
* Updates the value to the map. If there already is a value the new value is the minimum from  
* old and new values.
* @param P		The position of the updated height
* @param height	The height to be updated
* @return the updated value	
*/
float TGroundModelMap::updateMinimumHeight(TCoord2D P, float height){
	TBasePixel pix;
	float h;

	
	pix = this->getValue(P);
	if(pix.v == -1){
		h=height;
	}
	else{
		h = this->getHeight(P);
		if(h > height) h = height; 
	}
	
	this->setHeight(P,h);

	return h;

}

/**
 * Updates the value to the map. If there already is a value the new value is the Maximum from  
 * old and new values.
 * @param P		The position of the updated height
 * @param height	The height to be updated
 * @return the updated value	
 */
float TGroundModelMap::updateMaximumHeight(TCoord2D P, float height){
		TBasePixel pix;
		float h;

	
		pix = this->getValue(P);
		if(pix.v == -1){
				h=height;
		}
		else{
				h = this->getHeight(P);
				if(height > h) h = height; 
		}
	
		this->setHeight(P,h);

		return h;

}


/**
* Returns an average value that surround the point @P 
* @param P		The point from the grid around which the average is calculated
* @param radius	The radius, whis is taken into account in average calculation
* @return the average
*/
float TGroundModelMap::getAverage(TCoord2D P, float radius){
	float i,j;
	TPixelCoord c;
	int cnt = 0;
	float ave=0;

		
	/// X-coordinates
	for(i=P.getX()-radius;i<P.getX()+radius;i+=this->reso){
		// Y-coordinates
		for(j=P.getY()-radius;j<P.getY()+radius;j+=this->reso){
			if( fabs(i-P.getX())<(this->reso/2.0f) && fabs(j-P.getY())<(this->reso/2.0f)){
			}
			else{
				ave+=this->getHeight(TCoord2D(i,j));
				cnt++;
			}
		
		}
	}/// For x
	if(cnt == 0) return getHeight(P);
	return (ave/(float)cnt);
}


/**
* Returns an average value that surround the point @P 
* @param P		The point from the grid around which the average is calculated
* @param radius	The radius, whis is taken into account in average calculation
* @return the average
*/
float TGroundModelMap::getAverage(TPixelCoord P, float radius){
	int i,j;
	TPixelCoord c;
	int cnt = 0;
	float ave=0;
	int num;

	num = (int) (radius/this->reso);

	if(P.x > this->Nx || P.y > this->Ny){
		fprintf(stderr,"TGroundModelMap::getAverage:: Invalid pixel (%d,%d)\n",P.x,P.y);
	}
		
	/// X-coordinates
	for(i=P.x-num;i<P.x + num ; i++ ){
		// Y-coordinates
		for(j=P.y -num ;j<P.y + num;j++){
			
			if(i>= 0 && j>=0 && i<this->Nx && j<=this->Ny){
				if(i == P.x && j==P.y  ){
				}
				else{
					float val;
					if(!isIndexOutOfBounds(i, j)){
						val = this->getHeight(TPixelCoord(i,j));
						ave+= val;
						cnt++;
					}
					
				}
			}
			
		}
	}/// For x
	
	if(cnt == 0) return getHeight(P);
	
	return (ave/(float)cnt);
}

/**
 * Returns min value that surround the point @P 
 * @param P		The point from the grid around which the average is calculated
 * @return the minimum
 */
float TGroundModelMap::getMin(TPixelCoord P){
		int i,j;
		TPixelCoord c;
		int cnt = 0;
		float min=10000;
		int num;

		num = 1;
	  /// X-coordinates
		for(i=P.x-num;i<P.x + num ; i++ ){
		// Y-coordinates
				for(j=P.y -num ;j<P.y + num;j++){
			
						if(i>= 0 && j>=0 && i<this->Nx && j<=this->Ny){
								if(i == P.x && j==P.y  ){
								}
								else{
										float val;
										if(!isIndexOutOfBounds(i, j)){
												val = this->getHeight(TPixelCoord(i,j));
												if(val<min) min = val;
												cnt++;
										}
								}
						}
				}
		}/// For x
	
		if(cnt == 0) return getHeight(P);
	
		return (min);
}

/**
* Sets line to map. 
* The line is discreticed and the whole line area is set to @height
* @param l		The line to be set
* @param height	The height the pixels are set 
*/
void TGroundModelMap::setLine2Map(CLine line, float height){
	TAngle a;
	TLine l;
	float dl,linelength,gridlinelength=0;
	TCoord2D p;
	
	
	linelength = line.getLineLength();
	
	l = line.getTLine();
	a = line.getLineAngle();

	p.setCoord(l.x1,l.y1);
	dl = this->reso;

	while( linelength >= gridlinelength){
		this->setHeight(p,height);
		p.addPolar(dl,a.getValue());
		gridlinelength += dl; 
	}
}


/***** circleImage member functions *****/

void TGroundModelMap::setCircle2Map ( int x, int y, int radius )
{
	TBasePixel p;
	
	p.v = 500;
  
		// set pixels (i,j) in image corresponding to given circle parameters
  int i, j, diff2, diff;
  for (i = x - radius; i <= x + radius; ++i) {
    diff2 = (radius * radius) - ((i - x) * (i - x));
    diff = (int) sqrt(((double) diff2));
    j = y - diff;
    this->setAbsolutePixel(p,i,j);

    j = y + diff;
   this->setAbsolutePixel(p,i,j);
  }
}


/**
* Loads pointdata set from textfile and converts the data to the height map
* The data must be if form "%f %f %f\n" in order x,y,z; The Z is the height
* @param *filename		file to open
* @param zero_level		The value from the data which is set to zero level. Note 
*						that everything below this is set to zero. 
* @param max			The maximum value allowed in map
* /// FIXMEEE:: THIS HAS NOW IMPLEMENTATION THAT ONLY CONSIDERS THE DATA SET TAKEN FROM GIM HALL
*/
int TGroundModelMap::load3DTextDataset(const char *filename, float zero_level, float max){
	FILE *f;
	TCoord2D P;
	float x,y,z;
	
	f = fopen(filename,"rt");

	if(f==NULL){
		fprintf(stderr,"TGroundModelMap::load3DTextDataset: ERROR No such file as: %s\n",filename);
		return -1;
	}

	while(!feof(f)){
		fscanf(f,"%f %f %f\n",&x,&y,&z);
		z -= zero_level;
		
		if(z<0) z=0;
		if(z>max) z = 0;
		P.setCoord(x,y);

		if(x < -8 || x> 8 || y>8 || y<-8){
			this->updateMinimumHeight(P,z);
		}
		else{
			this->updateHeight(P,z);
		}
		if(x < -8 || x> 8 || y>8 || y<-6){
			if(z > 0.5){ this->setHeight(P,-0.5f);}
		}

		//this->updateMinimumHeight(P,z);
	}
	
	fclose(f);
	return 0;
}



int TGroundModelMap::interpolateGroundModelMap(){
	int i,j;
	int ii,jj;
	TBasePixel p;

	float dx,dy,w,w_sum,h_new;
	double r;
	TBasePixel pp;
	
	float *orig =(float *) malloc(Nx*Ny*sizeof(float));
	TBasePixel *ptr = getDataPtr();
	
	for(i=0;i<Nx*Ny;i++){
		orig[i] = (float) ptr[i].v;
	}
	
	float pix_val;
	float val;
	for(i=0;i<(int)this->Ny;i++){
		for(j=0;j<(int)this->Nx;j++){
			pix_val = orig[i*Nx + j];
			
			//if(p.v == -1){
				h_new = 0;
				w_sum = 0;
				for(ii=0;ii<(int)this->Ny;ii++){
					for(jj=0;jj<(int)this->Nx;jj++){
						
						dx = ((float) (jj-j)) * this->reso;
						dy = ((float) (ii-i)) * this->reso;
						r = dx*dx + dy*dy;
						
						if( r >1.0f) continue;
						
						w = (float)exp(-r / (0.2*0.2));
						val = orig[ii*Nx + jj];
						if(val != 0){
							h_new += w * val;
							w_sum += w;
						}//IF
					}//FOR JJ
				}// FOR II
				
				if(w_sum > 0.001){	
					h_new = h_new / w_sum;
					pp.v = (int)(h_new);
					this->setPixel(pp,j,i);
				}
				
			//}// if(p.v == 0){
			
		}// FOR J
		fprintf(stderr,"Processed row (%d)\n",i);
	}// FOR I
  free(orig);
	return 0;
}

int TGroundModelMap::interpolateGroundModelMap(int x,int y){
	//int i,j;
	int ii,jj;
	TBasePixel p;

	float dx,dy,w,w_sum,h_new;
	double r;
	TBasePixel pp;

	TBasePixel *ptr = getDataPtr();
	
	
	float pix_val;
	float val;
	
		pix_val = (float) ptr[y*Nx + x].v;
		
		//if(p.v == -1){
		h_new = 0;
		w_sum = 0;
		for(ii=0;ii<(int)this->Ny;ii++){
			for(jj=0;jj<(int)this->Nx;jj++){
					
				dx = ((float) (jj-x)) * this->reso;
				dy = ((float) (ii-y)) * this->reso;
				r = dx*dx + dy*dy;
					
				if( r >1.0f) continue;
					
				w = (float)exp(-r / (0.2*0.2));
				val = (float) ptr[ii*Nx + jj].v;
				if(val != 0){
					h_new += w * val;
					w_sum += w;
				}//IF
			}//FOR JJ
		}// FOR II
			
		if(w_sum > 0.001){	
			h_new = h_new / w_sum;
			pp.v = (int)(h_new);
			this->setPixel(pp,x,y);
		}

	return 0;
}


void TGroundModelMap::gradientFilter(float maxGrad){
	float *orig =(float *) malloc(Nx*Ny*sizeof(float));
	TBasePixel *ptr = getDataPtr();
	
	for(int i=0;i<Nx*Ny;i++){
		orig[i] = (float) ptr[i].v;
	}
	float *grad=NULL;
  int nx,ny;
	calculateGradient(grad,nx,ny);
	for(int i=0;i<Nx*Ny;i++){
		if(fabs(orig[i])>maxGrad){
			interpolateGroundModelMap(i%Nx,i/Nx);
		}
		if( ((i*10000/(Nx*Ny))%1000)==0) 
			fprintf(stderr,"%d percentages completed\n",(i*100/(Nx*Ny)));
	}
	
	free(grad);
  free(orig); 
}

/**
* saves the map to the text file
* @param *filename the file name to save to
*/
int TGroundModelMap::saveGroundModelMapToTextFile(const char *filename){
	FILE *f;
	int i,j;
	TBasePixel p;
	
	f = fopen(filename,"wt");
	fprintf(f,"size_x:%f\n",this->size_x);
	fprintf(f,"size_y:%f\n",this->size_y);
	fprintf(f,"reso:%f\n",this->reso);
	fprintf(f,"orig_x:%d\n",this->pix_origin.x);
	fprintf(f,"orig_y:%d\n",this->pix_origin.y);


	for(i=0;i<(int)this->Ny;i++){
		for(j=0;j<(int)this->Nx;j++){
			p = this->getPixel(j,i);
			fprintf(f,"%.2f ", (float)p.v/100.0f);
		}
		fprintf(f,"\n");
	}

	fclose(f);
	return 0;

}

/**
* saves the map to the text file for matlab, without any information about resolutio, or origins
* @param *filename the file name to save to
*/
int TGroundModelMap::saveGroundModelMapToTextFileForMatlab(const char *filename){
	FILE *f;
	int i,j;
	TBasePixel p;
	
	f = fopen(filename,"wt");

	for(i=0;i<(int)this->Ny;i++){
		for(j=0;j<(int)this->Nx;j++){
			p = this->getPixel(j,i);
			fprintf(f,"%.2f ", (float)p.v/100.0f);
		}
		fprintf(f,"\n");
	}

	fclose(f);
	return 0;

}
/**
* Loads the map from file, using the same format as @saveGroundModelMapToTextFile
* 
*/
int TGroundModelMap::loadFromTextFile(const char *filename){
	FILE *f;
	int i,j;
	TBasePixel p;
	float v;
	
	f = fopen(filename,"rt");
	if(f == NULL){
		fprintf(stderr,"TGroundModelMap::loadFromTextFile:: No such file as %s\n",filename);
		return -1;
	}
	fscanf(f,"size_x:%f\n",&this->size_x);
	fscanf(f,"size_y:%f\n",&this->size_y);
	fscanf(f,"reso:%f\n",&this->reso);
	fscanf(f,"orig_x:%d\n",&this->pix_origin.x);
	fscanf(f,"orig_y:%d\n",&this->pix_origin.y);

	this->initGroundModelMap(this->size_x,this->size_y,reso,pix_origin);

	fprintf(stderr,"size_x:%f\n",this->size_x);
	fprintf(stderr,"size_y:%f\n",this->size_y);
	fprintf(stderr,"reso:%f\n",this->reso);
	fprintf(stderr,"orig_x:%d\n",this->pix_origin.x);
	fprintf(stderr,"orig_y:%d\n",this->pix_origin.y);
		this->printBasicGridInfo(stderr);
	this->printMetricGridInfo(stderr);




	for(i=0;i<(int)this->Ny;i++){
		for(j=0;j<(int)this->Nx;j++){
			fscanf(f,"%f ", &v);
			p.v = (int) (v *100.0f); 
			this->setPixel(p,j,i);
			
		}
		fscanf(f,"\n");
	}
	
	fclose(f);
	
	return 0;
}

/**
* Computes a gradient of the height matrix
* Handles now all neccessary memory stuff for *out!
*** If *out == NULL the memory is reserved, else freed and reserved
*** Not efficient but easy ;)
* 
*
*/
int TGroundModelMap::calculateGradient(float *out, int &width, int &height, bool setToGrid){
		
		if(out) free(out);
		out = (float *) malloc(Nx*Ny*sizeof(float));
		width = Nx;
		height = Ny;
		int i;
		int j;
		for(j=0;j<Ny;j++){
				for(i=0;i<Nx;i++){
						out[j*Ny+i] = (getHeight(TPixelCoord(i,j))-getMin(TPixelCoord(i,j)))/this->reso;
						//if(out[j*Ny+i] > 0.5) out[j*Ny+i]=0.5;
						//else out[j*Ny+i] = 0;
						//out[j*Ny+i] = (getHeight(TPixelCoord(i,j))-getAverage(TPixelCoord(i,j),0.1))/this->reso;
				}///Nx
		}///j == Ny
		
		if(setToGrid){
				for(j=0;j<Ny;j++){
						for(i=0;i<Nx;i++){
								setHeight(TPixelCoord(i,j), out[j*Ny+i]);
						}
				}	
		}
		return 0;
}

#ifdef T_GROUND_MODEL_MAP_DEBUG

int main(void){
	TGroundModelMap *map;
	TPixelCoord tc;
	CLine line;
	FILE *f;
	float x1=0,x2=0,y1=0,y2=0;
	unsigned int i,j; 
	map = new TGroundModelMap();
	if(map->loadFromTextFile("groundMap.txt") ==-1){
		fprintf(stderr,"No map - exit!!!\n");
		exit(1);
	}
	
	///// Dataa pitää nostaa 1.9m 
	for(i=0;i<map->getNX();i++){
			for(j=0;j<map->getNY();j++){
					float val = map->getHeight(TPixelCoord(i,j));
					map->setHeight(TPixelCoord(i,j),val+1.9);	
			}
	}
	
	map->interpolateGroundModelMap();	
	
	
	
/*
	tc.origin(25.0f,25.0f,0.1f,(int)ORIGIN_CENTER);

	
	//map = new TGroundModelMap(25.0f, 25.0f, 0.1f, tc);
	map = new TGroundModelMap();
	
	if(map->loadFromTextFile("groundMap.txt") ==-1){
		map->initGroundModelMap(25.0f,25.0f,0.1f,tc);
		fprintf(stderr,"Loading the first 3D data set...");
		map->load3DTextDataset("xyz1.txt",-1.9f, 1.7f);
		fprintf(stderr,"Done.\n Loading the second one...");
		map->load3DTextDataset("xyz2.txt",-1.9f,1.7f);
		fprintf(stderr,"Done!\n");

		fprintf(stderr,"Interpolating....");
		map->interpolateGroundModelMap();
		fprintf(stderr,"Done\n");
		
		
		map->saveGroundModelMapToTextFile("groundMap.txt");	
	}
	
	//// For now for this very special case (the same goes for the >load3DTextDataset)
	/// Should somehow be generalized
	
	fprintf(stderr,"Averaging %d x %d grid\n",map->getNX(),map->getNY());
	for(i=0;i<map->getNX();i++){
		for(j=0;j<map->getNY();j++){
			if(map->getHeight(TPixelCoord(i,j))==-0.5){
				float ave;
				ave = map->getAverage(TPixelCoord(i,j),0.2f);
				if(ave >= 0){
					map->setHeight(TPixelCoord(i,j),ave);
				}
				else{
					map->setHeight(TPixelCoord(i,j),0);
				
				}
			}
			
		}
	}
	
	for(i=0;i<map->getNX();i++){
		for(j=0;j<map->getNY();j++){
			float ave;
			ave = map->getAverage(TPixelCoord(i,j),0.4f);
			map->setHeight(TPixelCoord(i,j),ave);	
		}
	}
	
	
	
	fprintf(stderr,"Setting the lines to the map\n");
	f = fopen("glines.txt","rt");
	fscanf(f," %f %f\n",&x1,&y1);
	
	  while(!feof(f)){
	  fscanf(f," %f %f\n",&x2,&y2);
	  line.setLine(x1,y1,x2,y2);
	  map->setLine2Map(line,1.0);
	  x1 = x2;
	  y1 = y2;
	  }
	  fclose(f);
	  
	 // map->setCircle2Map(0,0,20);
	*/
	map->saveGroundModelMapToTextFile("groundMap_edit.txt");	
	//fprintf(stderr,"Done! Finally saving for matlab!\n");
	//map->saveGroundModelMapToTextFileForMatlab("testi.txt");
return 0;
}

#endif
